#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <signal.h>
#include <iostream>
#include <string>
#include <map>
#include <fcntl.h>
#include <sys/select.h>
#include <pthread.h>

using namespace std;


class RingBuffer
{
    public:
        RingBuffer(uint32_t s, uint32_t c){
            head = 0;
            tail = 0;
            count = c;
            size = s;
        };
        ~RingBuffer(){
            if (start)
                delete []start;
        };
        bool createRingBuffer(){
            pthread_mutex_init(&not_empty_mutex, NULL);
            pthread_mutex_init(&not_full_mutex, NULL);
            pthread_condattr_t attr;
            pthread_condattr_init(&attr);
            pthread_condattr_setclock(&attr, CLOCK_MONOTONIC);
            pthread_cond_init(&not_empty_cond, &attr);
            pthread_cond_init(&not_full_cond, &attr);

            start = new (nothrow) char[size*count];
            if (start == NULL)
                return false;
            else
                return true;
        }
        bool isEmpty(){
            if (head == tail)
                return true;
            else
                return false;
        };
        bool isFull(){
            if (head == (tail + 1)%count)
                return true;
            else
                return false;
        };
        bool pushData(char *data, uint16_t len)
        {
            struct timespec ts;
            int res;
            char *buf = NULL;

            if (len > size - 2)
            {
                cout<<"data count is too big! data len="<<len<<",buf size="<<size - 2<<endl;
                return false;
            }

            clock_gettime(CLOCK_MONOTONIC, &ts);
            ts.tv_sec += 2;

            res = 0;
            pthread_mutex_lock(&not_full_mutex);
            if (isFull())
            {
                res = pthread_cond_timedwait(&not_full_cond, &not_full_mutex, &ts);
            }
            pthread_mutex_unlock(&not_full_mutex);
            
            if (res != 0)
            {
                cout<<"wait empty buffer timeout"<<endl;
                return false;
            }

            buf = start + tail * size;
            *(uint16_t *)buf = len;
            memcpy(buf + 2, data, len);

            pthread_mutex_lock(&not_empty_mutex);
            tail = (tail + 1) % count;
            pthread_cond_signal(&not_empty_cond);
            pthread_mutex_unlock(&not_empty_mutex);

            return true;
        };
        uint16_t pullData(char *data, uint16_t maxlen)
        {
            struct timespec ts;
            int res;
            char *buf = NULL;
            uint16_t len = 0;

            if ((data == NULL) || (maxlen < size))
                return 0;

            clock_gettime(CLOCK_MONOTONIC, &ts);
            ts.tv_sec += 2;

            res = 0;
            pthread_mutex_lock(&not_empty_mutex);
            if (isEmpty())
            {
                res = pthread_cond_timedwait(&not_empty_cond, &not_empty_mutex, &ts);
            }
            pthread_mutex_unlock(&not_empty_mutex);

            if (res != 0)
            {
                cout<<"wait buffer data timeout"<<endl;
                return 0;
            }

            buf = start + head * size;
            len = *(uint16_t *)buf;
            memcpy(data, buf + 2, len);

            pthread_mutex_lock(&not_full_mutex);
            head = (head + 1) % count;
            pthread_mutex_unlock(&not_full_mutex);

            pthread_cond_signal(&not_full_cond);

            return len;
        };

    private:
        char *start;
        uint16_t head;
        uint16_t tail;
        uint16_t count;
        uint16_t size;
        pthread_mutex_t not_empty_mutex;
        pthread_cond_t not_empty_cond;
        pthread_mutex_t not_full_mutex;
        pthread_cond_t not_full_cond;
};

#define RECV_BUF_SIZE 4096
int g_running = 1;

void print_format_data(char *buffer, int size)
{
    if (buffer == NULL || size <= 0) {
        printf("Invalid buffer or size.\n");
        return;
    }

    for (int i = 0; i < size; i++) {
        if (i % 16 == 0 && i != 0) {
            printf("\n");
        }
        printf("%02x ", (unsigned char)buffer[i]);
    }
    printf("\n");
}

void sig_handler(int signo)
{
    if (signo == SIGPIPE) {
        printf("Received SIGPIPE signal.\n");
    }
    
    if (signo == SIGINT) {
        printf("Received SIGINT signal.\n");
    }

    g_running = 0;

    return;
}

uint32_t swap32(uint32_t val)
{
    return ((val & 0x000000FF) << 24) |
           ((val & 0x0000FF00) << 8)  |
           ((val & 0x00FF0000) >> 8)  |
           ((val & 0xFF000000) >> 24);
}

float ntoh_f(char *pdata)
{
    union {
        float f;
        uint32_t i;
    } u;

    u.i = swap32(ntohl(*(uint32_t*)pdata));
    return u.f;
}

bool analysis_frame(char *buf, int size)
{
    char * point = buf;
    uint32_t chan_num = 0;
    uint32_t length;
    uint32_t code;

    if (size < 16){
        printf("received data size is too short:%d\n",size);
        return false;
    }

    string str = string(point, 8);
    if (str != "KMSD2017"){
        return false;
    }
    point += 8;

    code = swap32(ntohl(*(uint32_t *)point));
    point += 4;

    length = swap32(ntohl(*(uint32_t *)point));
    point += 4;
    if (length != size)
    {
        printf("received data size is not fit to length in frame, size=%d, length=%d\n",size,length);
        return false;
    }

    if (code != 16)
    {
        point += length - 16;
        return false;
    }

    chan_num = 0;
    printf("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    printf("Received data at %ld.%03ld seconds, len: %d\n", ts.tv_sec, ts.tv_nsec/1000000, size);
    cout<<"time second:"<<swap32(ntohl(*(uint32_t *)point))<<endl;
    point += 4;
    cout<<"time nsecond:"<<swap32(ntohl(*(uint32_t *)point))<<endl;
    point += 4;
    cout<<"APPID:"<<std::hex<<swap32(ntohl(*(uint32_t *)point))<<endl;
    point += 4;
    cout<<std::dec;
    cout<<"channel count:"<<swap32(ntohl(*(uint32_t *)point))<<endl;
    chan_num = swap32(ntohl(*(uint32_t *)point));
    point += 4;
    for (uint32_t i = 0; i < chan_num; i++)
    {
        cout<<"channel "<<i<<" val:"<<ntoh_f(point)<<"(float)"<<endl;
        point += 4;
        cout<<"channel "<<i<<" angle:"<<ntoh_f(point)<<"(float)"<<endl;
        point += 4;
    }
    printf("----------------------------------------------------------\n");

    return true;
}

RingBuffer ring(2048, 10);

void * frame_process(void *para)
{
    char frame[2048];
    uint16_t len;
    while(g_running)
    {
        if ((len = ring.pullData(frame, sizeof(frame))) == 0)
            continue;
        
        if (analysis_frame(frame, len)) {
            printf("Frame analysis success++++++++++.\n");
        } else {
            printf("Frame analysis failed-----------.\n");
            //print_format_data(rev_buf, str_len);
        }  
    }

    return NULL;
}

int main(int argc, char *argv[])
{
    if (argc != 2){
        printf("Usage: %s <server_ip>\n", argv[0]);
        return -1;
    }

    int client_socket_fd = -1;
    int str_len;
    struct sockaddr_in serv_addr;
    pthread_t thd_id;
    int locate_head_stage = 0;
    char * recv_buf = NULL;
    uint32_t maxlen = 1;
    char header[16] = {0};
    uint32_t code = 0;
    uint32_t length = 0;
    uint32_t offset = 0;
    const char *start_header = "KMSD2017";

    

    signal(SIGPIPE, sig_handler);
    signal(SIGINT, sig_handler);   

    client_socket_fd = socket(PF_INET, SOCK_STREAM, 0);
    if(client_socket_fd < 0)
    {
        printf("Failed to create socket: %s\n", strerror(errno));
        return -1;
    }
    printf("create socket\n");

    if (!ring.createRingBuffer())
    {
        printf("Failed to create ringbuf\n");
        close(client_socket_fd);
        return -1;
    }
    printf("create ringbuffer\n");


    pthread_create(&thd_id, NULL, frame_process,NULL);
    fcntl(client_socket_fd, F_SETFL, fcntl(client_socket_fd, F_GETFL, 0) | O_NONBLOCK);

    printf("create pthread\n");
    memset(&serv_addr, 0, sizeof(serv_addr));
    serv_addr.sin_family = AF_INET;
    serv_addr.sin_addr.s_addr = inet_addr(argv[1]);
    serv_addr.sin_port = htons(5023);

    int conn_res = 0;
    while((conn_res = connect(client_socket_fd, (struct sockaddr*)&serv_addr, sizeof(serv_addr))) <0)
    {
        if (!g_running)
            goto EXIT;
        sleep(1);        
    }

    printf("Connected to server %s\n", argv[1]);

    recv_buf = new (std::nothrow)char[RECV_BUF_SIZE];
    if (recv_buf == NULL)
    {
        printf("Failed to allocate memory for recv_buf\n");
        close(client_socket_fd);
        return -1;
    }

    fd_set readfds;

    while(g_running)
    {
        FD_ZERO(&readfds);
        FD_SET(client_socket_fd, &readfds);
        struct timeval tv = {5,0};

        int res = select(client_socket_fd + 1, &readfds, NULL, NULL, &tv);
        if (res < 0)
        {
            printf("select error, errno=%d\n", errno);
            g_running = 0;
            break;
        }
        else if (res == 0)
        {
            printf("select timeout\n");
        }
        else{
            str_len = recv(client_socket_fd, recv_buf + offset, maxlen, 0);
            if  (str_len < 0) {
                printf("Failed to receive data: %s\n", strerror(errno));
                g_running = 0;
                break;
            } else if (str_len == 0) {
                printf("Connection closed by server.\n");
                g_running = 0;
                break;
            }
            else{
                if (str_len < maxlen)
                {
                    maxlen -= str_len;
                    offset += str_len;
                    continue;
                }

                maxlen = 0;
                offset += str_len;
                
                switch(locate_head_stage){
                    case 0 ... 6:
                        if (recv_buf[locate_head_stage] == start_header[locate_head_stage])
                        {
                            locate_head_stage++;
                        }
                        else{
                            locate_head_stage = 0;
                            offset = 0;
                        }
                        maxlen = 1;
                        break;
                    case 7:
                        if (recv_buf[locate_head_stage] == start_header[locate_head_stage])
                        {
                            locate_head_stage = 8;
                            maxlen = 8;
                        }
                        else{
                            locate_head_stage = 0;
                            offset = 0;
                            maxlen = 1;
                        }
                        break;
                    case 8:
                        if (memcmp(recv_buf, "KMSD2017", 8) != 0)
                        {
                            locate_head_stage = 0;
                            maxlen = 1;
                            offset = 0;
                            break;
                        }

                        locate_head_stage = 9;
                        code = swap32(ntohl(*(uint32_t *)(recv_buf + 8)));
                        length = swap32(ntohl(*(uint32_t *)(recv_buf + 12)));
                        maxlen = length - 16;
                        break;
                    case 9:
                        locate_head_stage = 8;
                        maxlen = 16;
                        offset = 0;

                        if ((code != 16) || (length > 2040))
                        {
                            printf("Received data code is not 16, code=%d, length=%d\n", code, length);
                            continue;
                        }

                        if (!ring.pushData(recv_buf, length))
                        {
                            printf("push data to ring error\n");
                        }
                        break;
                    default:
                        break;
                }
            }
        }
    }
    pthread_join(thd_id, NULL);
EXIT:
    close(client_socket_fd);
    printf("thread sv client exit\n");

    return 0;
}
